/**
 * @file LaserData.h
 * @author  
 * @brief 激光雷达数据
 * @version 1.0
 * @date 2020-07-23
 * 
 * @copyright Copyright (c) 2020
 * 
 */

#pragma once
#include "data_struct/robot/Pose.h"

namespace behavior_controller
{
    class LaserData
    {
    public:
        LaserData(){};
        LaserData(int _id, double _angle_min, double _angle_max, double _angle_increment,
                  double _range_min, double _range_max, const std::vector<double> &_ranges,
                  const std::vector<double> &_intensities, PoseF _lidar_robot_pose)
            : id_(_id),
              angle_min_(_angle_min),
              angle_max_(_angle_max),
              angle_increment_(_angle_increment),
              range_min_(_range_min),
              range_max_(_range_max),
              ranges_(_ranges),
              intensities_(_intensities),
              lidar_robot_pose_(_lidar_robot_pose){};
        /**
         * @brief 判断改点是否是有效点
         * @param i 当前点在容器中的位置
         * @return 是否有效点
         *
         */
        bool isOk(int i) const
        {
            return isValidRange(ranges_[i], range_min_, range_max_);
        };
        /**
         * @brief 是否有效距离
         * 有效距离： range_min <= range <= range_max
         * @return bool
         */
        bool isValidRange(const double &range,
                          const double &range_min,
                          const double &range_max) const
        {
            if (range < range_min)
            {
                return false;
            }
            if (range > range_max)
            {
                return false;
            }
            if (std::isnan(range) || !std::isfinite(range))
            {
                return false;
            }
            return true;
        };

        /**
         * @brief cout<<输出格式
         * @return 是否有效点
         */
        friend std::ostream &operator<<(std::ostream &outs, const LaserData &_d)
        {
            //        outs << _d.id_ << " ";
            outs << _d.angle_min_ << " " << _d.angle_max_ << " "
                 << _d.angle_increment_ << std::endl;
            outs << _d.range_max_ << " " << _d.range_min_ << std::endl;
            outs << _d.ranges().size() << std::endl;

            for (size_t i = 0; i < _d.ranges().size(); i++)
            {
                outs << _d.ranges_[i] << " ";
            }
            outs << std::endl;

            outs << std::endl;
            return outs;
        };

        /**
         * @brief 得到当前坐标系下的雷达点位置
         * @param i 第几个点
         * @param pose 需要转换成坐标系的坐标点
         * @return point类型的点
         */
        PointF GetPoint(const PoseF &pose, size_t i) const
        {
            PointF tmp_pose;
            // return pose.TransformFromPolarBaseonCurOrigin(
            //     ranges_[i], angle_min_ + i * angle_increment_);
            return tmp_pose;
        }
        /**
         * @brief 获取极坐标
         * 参考系：激光雷达坐标系
         * @param i 索引
         * @param angle 角度 （弧度）
         * @param range 距离 （米）
         * @return void
         */
        void GetPolarCoor(size_t i, double &angle, double &range) const
        {
            range = ranges_[i];
            angle = angle_min_ + i * angle_increment_;
        }

        //     /**
        //  * @brief 更新scan
        //  */
        //     void UpdateLaserData(int _id, double _angle_min, double _angle_max,
        //                         double _angle_increment, double _range_max,
        //                         double _range_min, const std::vector<double> &ranges,
        //                         const std::vector<double> &intensities,
        //                         const PoseF _lidar_pose);

        //     /**
        //  * @brief 更新scan
        //  */
        //     void UpdateLaserData(int _id, double _angle_min, double _angle_max,
        //                         double _angle_increment, double _range_max,
        //                         double _range_min, std::vector<double> &&ranges,
        //                         std::vector<double> &&intensities,
        //                         const PoseF _lidar_pose);

        /**
         * @brief 返回当前雷达数据的机器人位置
         * @return
         */
        PoseF robot_pose() const { return lidar_robot_pose_; }

        /**
         * @brief 返回当前雷达的id
         * @return
         */
        int id() const { return id_; }

        /**
         * @brief 返回当前雷达的起始角度
         * @return
         */
        double angle_min() const { return angle_min_; }

        /**
         * @brief 返回当前雷达的结束角度
         * @return
         */
        double angle_max() const { return angle_max_; }

        /**
        * @brief 返回当前雷达相隔点的间隔角度
        * @return
        */
        double angle_increment() const { return angle_increment_; }

        /**
        * @brief 返回当前雷达最大有效距离
        * @return
        */
        double range_max() const { return range_max_; }

        /**
        * @brief 返回当前雷达最小有效距离
        * @return
        */
        double range_min() const { return range_min_; }

        /**
        * @brief 返回当前雷达最小有效距离
        * @return
        */
        int length() const { return ranges_.size(); }

        /**
        * @brief 返回当前雷达的所有点距离
        * @return
        */
        const std::vector<double> &ranges() const { return ranges_; }

        /**
        * @brief 返回当前雷达的所有点的光强
        * @return
        */
        const std::vector<double> &intensities() const { return intensities_; }

    private:
        int id_;                         //激光雷达时间戳序号
        double angle_min_;                //检测最小角
        double angle_max_;                //检测最大角
        double angle_increment_;          //角分辨率
        double range_min_;                //距离最小值
        double range_max_;                //距离最大值
        std::vector<double> ranges_;      //各点距离序列
        std::vector<double> intensities_; //各点亮度值序列
        PoseF lidar_robot_pose_;         //激光雷达的世界坐标系位置
    };
} // namespace behavior_controller